# -*- coding: utf-8 -*-
"""
Created on Fri Sep  6 10:22:26 2024

@author: LENOVO
"""

# -*- coding: utf-8 -*-
"""
Created on Thu Sep  5 19:36:07 2024

@author: LENOVO
"""

import matplotlib.pyplot as plt
import numpy as np
from sympy import *
from scipy.optimize import root, fsolve
import pandas as pd

#常数的确定
a=16*55/100  #m
b=55/(2*np.pi)/100 #m
vh=1 #m/s
L0=(341-27.5*2)/100
Lb=(220-27.5*2)/100
c=442.4836510502367 #待定系数C
# theta=np.linspace(0,32*np.pi,30*180)
r=lambda theta:((a-b*(theta)))

f0=lambda theta:a-b*(theta)-4.5
jie=fsolve(f0,49)
print(jie)
# ax=plt.subplot(111, polar=True)
# ax.set_theta_direction(-1)
# plt.plot(theta,r(theta),lw=1,c='r', label='原始数据点')
# plt.legend()
# plt.title('非线性最小二乘拟合')
# plt.grid()  
# plt.show()

#求解theta与t的关系
# var('theta')
# t=integrate(sqrt((a-b*theta)**2+b**2),theta)
# print(t)

# print("FENGE")
# theta=symbols('theta')
# t=Function("t")
# eq1=diff(t(theta),theta,1)-sqrt(((a-b*theta)**2+b**2))
# s1=dsolve(eq1,t(theta))
# print(s1)


s=lambda theta:a*theta-b*theta**2/2

thetas=lambda t:(a-np.sqrt(a**2-2*b*vh*t))/b #猜测角度值

def theta(t):
    f=lambda x:(b*x-a)*np.sqrt((a-b*x)**2+b**2)/(2*b)-0.5*b*np.log(np.sqrt((a-b*x)**2+b**2)+a-b*x)+442.4836510502367-t
    theta=root(f,thetas(t))
    theta=theta.x[0]
    return theta

# plt.plot(t,s(theta(t)),lw=1,c='r', label='原始数据点')
# plt.plot(t,theta(t),lw=1,c='r', label='原始数据点')
# plt.plot(t,r(theta(t)),lw=1,c='r', label='原始数据点')
# plt.legend()
# plt.title('非线性最小二乘拟合')
# plt.grid()  
# plt.show()



#循环遍历
# for i in range(300,-1,-1):    #300s到0s循环遍历
#第i秒位置函数

def position(i):
    Data=[]


    THETA=[]
    X=[]
    Y=[]
    # y=np.ones((301,1))
    
    THETA.append(theta(i))
    X.append(r(THETA[0])*np.cos((THETA[0])))
    Y.append(-r(THETA[0])*np.sin((THETA[0])))

    Data.append(theta(i))
    f=lambda thetai:(r(THETA[0]))**2+(r(thetai))**2-L0**2-2*(r(THETA[0]))*(r(thetai))*np.cos(THETA[0]-thetai)
    thetai=root(f,THETA[0]-0.5)
    thetai=thetai.x[0]
    
    # print("theta1:",thetai)
    THETA.append(thetai)
    X.append(r(THETA[1])*np.cos((THETA[1])))
    Y.append(-r(THETA[1])*np.sin((THETA[1])))
    Data.append(thetai)
    
    for j in range(0,222,1):
        
        f=lambda thetai:(r(THETA[1+j]))**2+(r(thetai))**2-Lb**2-2*(r(THETA[1+j]))*(r(thetai))*np.cos(THETA[1+j]-thetai)
        thetai=root(f,THETA[1+j]-0.5)
        thetai=thetai.x[0]
        THETA.append(thetai)
        X.append(r(THETA[j+2])*np.cos((THETA[j+2])))
        Y.append(-r(THETA[j+2])*np.sin((THETA[j+2])))
        # if THETA[i-2-j]<=0:
        #     N=j+1
        # if THETA[i-2-j]>=0:
        #     Data.append(thetai)
        # else:
        #     print("龙身进入了:",j+1)
        #     break
    # print(Data)

    # print(position)
    # X=position[:,0]
    # Y=position[:,1]
    # THETA=[x for x in THETA if(x!=1)]
    # position=[(x,y) for (x,y) in position if (x!=1 and y!=1)]
    # X=[x for x in X if(x!=1)]
    # Y=[x for x in Y if(x!=1)]
    # THETA=np.array(THETA)
    # X=np.array(X)
    # Y=np.array(Y)
    # THETA=THETA.reshape(-1,1)
    # X=X.reshape(-1,1)
    # Y=Y.reshape(-1,1)
    # # position=position.reshape(-1,1)
    A=np.column_stack((X, Y, THETA))
    # print(THETA)
    return A
WZ=position(300)
# print(WZ)    
# N=[]
# for k in range (0,301,1):
#     N.append(position(k))
# print(N)

# #导出到excel
# for k in range (0,301,1):

    
# df.to_excel('output.xlsx',index=False)

# df=pd.DataFrame(position(300))
# df.to_excel('300position.xlsx',index=False)
    
    
    
# f=lambda theta1:(r(theta(300)))**2+(r(theta1))**2-L0**2-2*(r(theta(300)))*(r(theta1))*np.cos(theta(300)-theta1)
# theta1=fsolve(f,43)
# print("theta1:",theta1[0])
# THETA=[]
# THETA.append(theta1)
# print(theta(300))
# print(s(theta(300)))


def Slope(ta):
    k=(-a*np.cos(ta)+b*np.sin(ta)+b*ta*np.cos(ta))/(-a*np.sin(ta)-b*np.cos(ta)+b*ta*np.sin(ta))
    return k

#速度的求解 #第i秒时候所有的速度
def Velocity(i):
    P=position(i)
    V=[]
    V.append(1)
    for j in range(1,224):
        L=np.array([P[j-1,0]-P[j,0],P[j-1,1]-P[j,1]])
        H=np.array([1/np.sqrt(1**2+(Slope(P[j-1,2]))**2),Slope(P[j-1,2])/np.sqrt(1**2+Slope(P[j-1,2])**2)])
        T=np.array([1/np.sqrt(1**2+(Slope(P[j,2]))**2),Slope(P[j,2])/np.sqrt(1**2+Slope(P[j,2])**2)])
        # V1=np.array([1,H])
        # V2=np.array([v,T])
        f=lambda v:abs((V[j-1]*H[0]*L[0]+V[j-1]*H[1]*L[1]))-abs((v*T[0]*L[0]+v*T[1]*L[1]))
        v=root(f,V[j-1]-0.01)
        v=v.x[0]
        V.append(v)
    return V


# V=Velocity(200)


def Storage (i):
    p=position(i)
    speed=Velocity(i)
    Info=np.column_stack((p[:,0], p[:,1], speed))
    return Info
    
Info=Storage(0)
datasheet = pd.read_excel('result1.xlsx',sheet_name="速度")
com=datasheet.values[:,0]
datasheet.values[:,1]=Info[:,2]
datasheet.to_excel('output.xlsx',index=False,header=False)
# print(com)
for k in range(1,302):
    df=[]
    df.append(com)

    # Info=Storage(k)
    # df[k]=pd.DataFrame(Info[:,2])
    # df=pd.concat([df[k-1],df[k]],axis=1)
    
    
    # df = pd.read_excel('result1.xlsx')



# df=pd.DataFrame(Info[:,2])
# df.to_excel('result1.xlsx', sheet_name="速度",index=False,header=False)

##论文表格
LT=[]
LS1=[]
LS51=[]
LS101=[]
LS151=[]
LS201=[]
LW=[]
for i in range(0,360,60):
    S=Storage(i)
    LT.append(S[0,2])
    LS1.append(S[1,2])
    LS51.append(S[51,2])
    LS101.append(S[101,2])
    LS151.append(S[151,2])
    LS201.append(S[201,2])
    LW.append(S[223,2])

LT=pd.DataFrame(LT)
LS1=pd.DataFrame(LS1)
LS51=pd.DataFrame(LS51)
LS101=pd.DataFrame(LS101)
LS151=pd.DataFrame(LS151)
LS201=pd.DataFrame(LS201)
LW=pd.DataFrame(LW)
df=pd.concat([LT,LS1,LS51,LS101,LS151,LS201,LW],axis=1)
df.to_excel('论文速度(改2.0).xlsx',index=False,header=False)

# WEIZHI=[]
# for i in range(0,360,60):
#     S=Storage(i)
#     WEIZHI.append(S[0,0])
#     WEIZHI.append(S[0,1])
#     WEIZHI.append(S[1,0])
#     WEIZHI.append(S[1,1])
#     WEIZHI.append(S[51,0])
#     WEIZHI.append(S[51,1])
#     WEIZHI.append(S[101,0])
#     WEIZHI.append(S[101,1])
#     WEIZHI.append(S[151,0])
#     WEIZHI.append(S[151,1])
#     WEIZHI.append(S[201,0])
#     WEIZHI.append(S[201,1])
#     WEIZHI.append(S[223,0])
#     WEIZHI.append(S[223,1])

# WEIZHI=np.array(WEIZHI)
# WEIZHI=WEIZHI.reshape(6,14)
# WEIZHI=WEIZHI.transpose()
# print(WEIZHI)
# weizhi=pd.DataFrame(WEIZHI)
# weizhi.to_excel('论文位置.xlsx',index=False,header=False)


#附件位置
# TWZ=[]
# for i in range(0,301,1):
#     S=Storage(i)
#     for j in range(0,224):       
#         TWZ.append(S[j,0])
#         TWZ.append(S[j,1])
# TWZ=np.array(TWZ)
# TWZ=TWZ.reshape(301,448)
# TWZ=TWZ.transpose()
# TWZ=pd.DataFrame(TWZ)
# TWZ.to_excel("附件位置.xlsx",index=False,header=False)

# #附件速度
# TV=[]
# for i in range(0,301,1):
#     S=Storage(i)
#     for j in range(0,224): 
#         TV.append(S[j,2])
# TV=np.array(TV)
# TV=TV.reshape(301,224)
# TV=TV.transpose()
# TV=pd.DataFrame(TV)
# TV.to_excel("附件速度(改）.xlsx",index=False,header=False)
    

    




